addpath('Functions');
r_num = 4; duration = 1000; MonteCarloNum = 50;
savepath = ['Results/StatisticalResults/', num2str(r_num),'robot/'];
if ~isfolder(savepath)
    mkdir(savepath);
end

pos_his = cell(MonteCarloNum,1);con_his = cell(MonteCarloNum,1);
parfor i = 1:MonteCarloNum
    tic
    [pos_his{i},con_his{i}] = runScenarioOnce(r_num,duration);
    toc
end

save([savepath,'pos_his.mat'],'pos_his');
save([savepath,'con_his.mat'],'con_his');

%%
% compute the range of coverage for each robot at the x-y plane
warning('off');field_limits = [0 100 0 100 0 20];
discretization_num = 50; mesh_size = field_limits(2)/discretization_num;
x_space = linspace(field_limits(1),field_limits(2),discretization_num);
y_space = linspace(field_limits(3),field_limits(4),discretization_num);
[X_points,Y_points] = meshgrid(x_space,y_space);
points_xy = [reshape(X_points,discretization_num^2,1) reshape(Y_points,discretization_num^2,1)];

robot = pos_his{1}{end};
centers = [50 50]; c_index = 1;
d_center = centers(c_index,:);
R = densityFunc(d_center,points_xy);
h = robot(:,end); sensor_para = 20;
[r,dr_dh] = coverageModel(h,sensor_para,'reciprocal');

H = zeros(MonteCarloNum,duration);
for i = 1:MonteCarloNum
    parfor j = 1:duration
            robot = pos_his{i}{j };
            H(i,j) = computeObj_vecform(robot,points_xy,R,mesh_size,r);      % initial cost
    end
end

save([savepath,'Objective.mat'],'H');

%%
r_num = 4;
savepath = ['Results/StatisticalResults/', num2str(r_num),'robot/'];
close all; clc; addpath('Functions/'); addpath('distributionPlot/');addpath('shaded_plots/')
statis = figure();
plot_distribution_max_min(1:1000,abs(H(:,1:1000))); hold on;
plot(1:1000,ones(1000,1)*5.2521); axis([1,1000,0,5.5]);
xlabel('time steps'); ylabel('Overall objective function');
text(930,1.8,'higher pattern'); text(930,1.4,'lower pattern');text(930,3.3,'global pattern');
ax1 = axes('Position',[0.5 0.2 .34 .4],'Box','on'); 

distributionPlot(abs(H(:,980:1000)),'widthDiv',[2 1],'histOri','left','color','b','showMM',0,'histOpt',1);
set(ax1,'NextPlot','add');
plot(1:21,ones(21,1)*5.2521,'r');
axis([1,21,5.244 5.253]);
set(ax1,'xticklabel',{'980',' ', ' ', ' ', ' ',......
                                  '985',' ', ' ', ' ', ' ',......
                                  '990',' ', ' ', ' ', ' ',......
                                  '995',' ', ' ', ' ',' ', '1000'});
                              
set(statis,'position',[1 1 860*2 300*2]);

saveas(statis,[savepath, num2str(r_num),'distribtuion.eps'],'epsc');
%% 5.2464
close all;clc;
field_limits = [0 100 0 100 0 20];
robot = pos_his{2}{end};
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h2 = figure('Name','CoverageAreaBeforeOptimization'); hold on; grid on;
pcolor(X,Y,Z); shading interp;colorbar; hold on;
view(2);caxis manual; caxis([0 1]); axis(field_limits)
axis(field_limits(1:4));
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(h2,[savepath,'pattern1_higher.eps'],'epsc');

% 5.2477
robot = pos_his{31}{end};
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h3 = figure('Name','CoverageAreaBeforeOptimization'); hold on; grid on;
pcolor(X,Y,Z); shading interp; colorbar; hold on;
view(2);caxis manual; caxis([0 1]); axis(field_limits)
axis(field_limits(1:4));
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(h3,[savepath,'pattern2_lower.eps'],'epsc');

%
robot = [42.9723686393179,46.7359246225410,9.39659666962345;56.5791937075619,41.7364623749410,7.48084565515741;46.9484374887256,61.0988453242048,7.02265347925409;59.9625856139019,55.0631519204984,3.75319474654044];
[X,Y,Z] =  computeMeshforPlot(robot,field_limits,200);
h4 = figure('Name','CoverageAreaBeforeOptimization'); hold on; grid on;
pcolor(X,Y,Z); shading interp; colorbar; hold on;
view(2);caxis manual; caxis([0 1]); axis(field_limits)
axis(field_limits(1:4));
ax = gca;
outerpos = ax.OuterPosition;
ti = ax.TightInset; 
left = outerpos(1) + ti(1);
bottom = outerpos(2) + ti(2);
ax_width = outerpos(3) - ti(1) - ti(3);
ax_height = outerpos(4) - ti(2) - ti(4);
ax.Position = [left bottom ax_width ax_height];
saveas(h4,[savepath,'globalOptimal.eps'],'epsc');

